//
// Created by augtons on 2022/1/10.
//

#include "windswing_specific.h"
#include "imu_windswing.h"

void initWindSwing(){
    spinlock_initialize(&windSwingStatus.spinlock);
    initWindSwingIMU();
    currentTask = NULL;
//    startWindswing();
}

void set_tileAngle(float tileAngle){
    taskENTER_CRITICAL(&windSwingStatus.spinlock);//进入临界区
    {
        windSwingStatus.tileAngle = tileAngle;
    }
    taskEXIT_CRITICAL(&windSwingStatus.spinlock);//退出临界区
}

void set_azimuth(float azimuth){
    taskENTER_CRITICAL(&windSwingStatus.spinlock);
    {
        windSwingStatus.azimuth = azimuth;
    }
    taskEXIT_CRITICAL(&windSwingStatus.spinlock);
}

float get_tileAngle(){
    float angle;
    taskENTER_CRITICAL(&windSwingStatus.spinlock);//进入临界区
    {
        angle = windSwingStatus.tileAngle;
    }
    taskEXIT_CRITICAL(&windSwingStatus.spinlock);//退出临界区
    return angle;
}

float get_azimuth(){
    float azimuth;
    taskENTER_CRITICAL(&windSwingStatus.spinlock);
    {
        azimuth = windSwingStatus.azimuth;
    }
    taskEXIT_CRITICAL(&windSwingStatus.spinlock);
    return azimuth;
}

void set_eularAngle(EulerAngle *eulerAngle) {
    taskENTER_CRITICAL(&windSwingStatus.spinlock);
    {
        windSwingStatus.eulerAngle.pitch = eulerAngle->pitch;
        windSwingStatus.eulerAngle.roll = eulerAngle->roll;
        windSwingStatus.eulerAngle.yaw = eulerAngle->yaw;
    }
    taskEXIT_CRITICAL(&windSwingStatus.spinlock);
}

void get_eularAngle(EulerAngle *eulerAngle) {
    taskENTER_CRITICAL(&windSwingStatus.spinlock);
    {
        eulerAngle->pitch = windSwingStatus.eulerAngle.pitch;
        eulerAngle->roll = windSwingStatus.eulerAngle.roll;
        eulerAngle->yaw = windSwingStatus.eulerAngle.yaw;
    }
    taskEXIT_CRITICAL(&windSwingStatus.spinlock);
}

static void pwm_gen2(float pwmx, float pwmy);

static void pwm_gen(float angle){ //l nishizhen
    const float pi_180 = 0.017453292f;

    float r = motoPwm.r;
    float l = motoPwm.l;

    float pwmx = r * cosf(pi_180 * angle) + l * cosf(pi_180 * (angle + 90));//0-180
    float pwmy = r * sinf(pi_180 * angle) + l * sinf(pi_180 * (angle + 90));  // -90 - 90

    pwm_gen2(pwmx, pwmy);
}

static void pwm_gen2(float pwmx, float pwmy) {
    float moto1 = 0;
    float moto2 = 0;
    float moto3 = 0;
    float moto4 = 0;

    if(pwmx > 0){
        moto4 = pwmx;
        moto2 = -pwmx/2;
    }else if(pwmx == 0){
        moto4 = 0;
        moto2 = 0;
    }else {
        moto4 = pwmx/2;
        moto2 = -pwmx;
    }

    if(pwmy > 0){
        moto3 = pwmy;
        moto1 = -pwmy/2;
    }else if(pwmy == 0){
        moto3 = 0;
        moto1 = 0;
    }else{
        moto3 = pwmy/2;
        moto1 = -pwmy;
    }

    moto_forward(0, moto1);
    moto_forward(1, moto2);
    moto_forward(2, moto3);
    moto_forward(3, moto4);
}


float simplifyAngle(float a){
    if(a >= -180 && a <= 180){
        return a;
    } else{
        if(a > 0){
            return a - (int)(a + 180.0f)/360 * 360;
        }else{
            return a + (int)(a - 180.0f)/360 * 360;
        }
    }
}

_Noreturn void task_line_pid(void *args);
_Noreturn void task_line(void *args){
    int step = 1;
    xTaskCreate(task_line_pid, "task_line_pid", BEHAVIOR_TASK_S, NULL, BEHAVIOR_TASK_P, &currentTask2);
    while (1){
        if(step == 1){
            motoPwm.r = 70;
//            pwm_gen(80, 0, (float)behavior_data);
            step = 2;
            vTaskDelay(pdMS_TO_TICKS(500));
        }else{
            step = 1;
            motoPwm.r = -70;
//            pwm_gen(80, 0, plusAngle((float)behavior_data, 180.0f));
//            step = 2;
            vTaskDelay(pdMS_TO_TICKS(500));
        }
    }
}

_Noreturn void task_line_pid(void *args){
    float e, de = 0, last_e = 0, Ie = 0, pid;
    const float Kp = 100.0f,Ki = 0.01f;
    float azimuth;
    while (1){
        azimuth = get_azimuth();
        if(azimuth > 0){        //e = azimuth - angle
            e = azimuth - (float)behavior_data;
        }else{
            e = azimuth - simplifyAngle((float)behavior_data + 180.0f);
        }
        Ie += e;
        de = e - last_e;

        pid = (Kp * e + Ki * Ie) * (float)(e>0);
        if(pid >= 30){
            pid = 30;
        }
        motoPwm.l = pid;
        pwm_gen((float)behavior_data);
        vTaskDelay(pdMS_TO_TICKS(10));
    }

}

_Noreturn void task_stop_self(void *args){
    float e, Ie = 0;
    float azimuth, tileAngle;
    float pid = 0, Kp = 20.0f, Ki = 0;
    while (1){
        azimuth = get_azimuth();
        tileAngle = get_tileAngle();
        e = tileAngle - 4.5f;
        Ie += e;
        if(e > 0){
            pid = Kp*(e) + Ki * Ie;
        }
//        if(pid >= 100){
//            pid = 50;
//        }else
        if(pid < 5){
            pid = 0;
        }

        motoPwm.l = pid;
        pwm_gen(simplifyAngle(azimuth));
        vTaskDelay(pdMS_TO_TICKS(10));
    }
}

_Noreturn void task_circle(void *args){
    EulerAngle eulerAngle;

    int64_t starttime = esp_timer_get_time();
    const float pipi_div_T = 0.006283f;

    float target_x;
    float target_y;

    float e1, Ie1 = 0;  //x
    float e2, Ie2 = 0;  //y
    float pidx = 0, pidy = 0;
    float Kpx = 150, Kix = 0;
    float Kpy = 80, Kiy = 0;

    int64_t delta_t;

    while (1) {
        // pitch: circle with X, and association with motos at Y
        // behavior_data max angle
        get_eularAngle(&eulerAngle);
        delta_t = (esp_timer_get_time() - starttime) / 1000;
        target_x = (float)behavior_data * cosf(pipi_div_T * delta_t);
        target_y = (float)behavior_data * sinf(pipi_div_T * delta_t);

        e1 = target_x - eulerAngle.roll;
        e2 = target_y - eulerAngle.pitch;

        Ie1 += e1;
        Ie2 += e2;

        pidx = Kpx * e1 + Kix * Ie1;
        pidy = Kpy * e2 + Kiy * Ie2;

        pwm_gen2(pidx, pidy);
        vTaskDelay(pdMS_TO_TICKS(5));
    }

//    float e1, Ie1 = 0;      // fa xiang   ;r
//    float pid = 0, Kp = 20.0f, Ki = 0;
//
//    float e2, Ie2 = 0;      // qie xiang  ;l
//    float pid2 = 0, Kp2 = 20.0f, Ki2 = 0;
//    float azimuth, tileAngle;
//    float targetTimeAngle;
//
//
//    int64_t startTime = esp_timer_get_time();
//    int64_t currentTime = 0;
//    while (1){
//        currentTime = esp_timer_get_time() - startTime;
//        targetTimeAngle = simplifyAngle(0.00036f * currentTime);
//        azimuth = get_azimuth();
//        tileAngle = get_tileAngle();
//
//        e1 = (float)behavior_data - tileAngle;
//        Ie1 += e1;
//        pid = Kp * e1 + Ki * Ie1;
//
//        e2 = simplifyAngle(targetTimeAngle - azimuth);
//        Ie2 += e2;
//        pid2 = Kp2 * e2 + Ki2 * Ie2;
//
//        if(pid >= 80){
//            motoPwm.r = 80;
//        }else if(pid <= -80){
//            motoPwm.r = -80;
//        }else{
//            motoPwm.r = pid;
//        }
//
//        if(pid2 >= 30){
//            motoPwm.l = 30;
//        }else if(pid2 <= -30){
//            motoPwm.l = -30;
//        }else{
//            motoPwm.l = pid2;
//        }
//        pwm_gen(azimuth);
//        vTaskDelay(pdMS_TO_TICKS(10));
//    }
}